#include "C_H.h"
#include "isr.h"

// �ٶ���ض���
uint8_t motor_test_flag = 0;
uint8_t speed_flag = 1;
float pid_output_l;
float pid_output_r;
float pid_motor_output;
float pid_speed_output_l,pid_speed_output_r;
float Speed_Diffen_xishu = 0.25f;          // ����ϵ��
float Speed_Goal = 150.0f;                 // Ŀ���ٶ�
float Speed_Goal_l = 0;                    // ����Ŀ���ٶ�
float Speed_Goal_r = 0;                    // ����Ŀ���ٶ�
float S3010_Duty = SERVO_PWM_DUTY_MAX / 2; // ���PWMֵ����ʼ��Ϊ��ֵ
int pwm = 1500;
uint8_t motor_cnt = 0;
float current_speed_l,current_speed_last_l;
float current_speed_r,current_speed_last_r;
float current_speed = 10;
uint8_t tcp_flag = 0;
int Right_encoder_data_kal = 0, Left_encoder_data_kal = 0;
int Left_encoder_data_kal_last = 0, Right_encoder_data_kal_last = 0;
// ������
static float target_speed = 0.0f;  // 初始目标速度（动态调整）
const float deceleration_rate = -0.1f; // 减速斜率（可调，负值表示减速）


float max_speed_l = 0;
float max_speed_r = 0;
float max_speed = 0;

int speed_setPoint_l,speed_setPoint_r;

int temp_r = 0,temp_l = 0;
int last_raw_r = 0,last_raw_l = 0;
float accumulated_distance = 0;

// 新增全局变量（用于平滑滤波，放在文件头部）
#define SMOOTH_WINDOW_SIZE 3  // 滑动平均窗口大小
static float speed_buffer[SMOOTH_WINDOW_SIZE] = {0};
static int speed_buffer_index = 0;
float current_speed_pinghua = 0;

float Distance=0;
void pit_callback()
{

    // 1. 读取编码器值（完全保持原有变量名和逻辑）
    current_speed_r = -encoder_get_count(ENCODER_1);  // 右轮（符号反转保留）
    current_speed_l = encoder_get_count(ENCODER_2);   // 左轮

    // 2. 卡尔曼滤波（先对原始编码器值进行滤波）
    Left_encoder_data_kal = kalman_update(&L_encoderkal, current_speed_l);
    Right_encoder_data_kal = kalman_update(&R_encoderkal, current_speed_r);

    // 3. 计算原始速度（保持原有变量名）
    current_speed = (Left_encoder_data_kal + Right_encoder_data_kal) / 2;

    // 4. 对current_speed进行平滑滤波（新增部分）
    speed_buffer[speed_buffer_index] = current_speed;
    speed_buffer_index = (speed_buffer_index + 1) % SMOOTH_WINDOW_SIZE;
    

    for (int i = 0; i < SMOOTH_WINDOW_SIZE; i++) {
        current_speed_pinghua += speed_buffer[i];
    }
    current_speed_pinghua /= SMOOTH_WINDOW_SIZE;

    // 5. 累计距离（使用平滑后的速度）
    accumulated_distance += (1.0) * current_speed_pinghua / 150;

    Distance+=(current_speed_pinghua/200);
    #ifdef GYRO
        imu_gyro_z = imu_get_raw(imu_file_path[GYRO_Z_RAW]);
    #endif


    // printf("%d\n\r", xie_shizi_flag_right);

    //adc
    adc_reg = adc_convert(ADC_REG_PATH);
    adc_scale = adc_get_scale(ADC_SCALE_PATH);
    battery_vol = adc_reg * adc_scale;

    //printf("adc_reg: %d, adc_scale: %f, battery_vol: %d\r\n", adc_reg, adc_scale, battery_vol);
    //printf("battery_vol: %d\r\n", battery_vol);
    

    // 记录最大值
    if (Left_encoder_data_kal > max_speed_l) {
        max_speed_l = Left_encoder_data_kal;
    }
    if (Right_encoder_data_kal > max_speed_r) {
        max_speed_r = Right_encoder_data_kal;
    }
    if (current_speed > max_speed) {
        max_speed = current_speed;
    }

    // 如果需要，可以在这里打印最大值
    if (zhushi_flag) {
        printf("Max Speed L: %f, Max Speed R: %f, Max Speed: %f\r\n", 
               max_speed_l, max_speed_r, max_speed);
    }


    if (run_flag == 2)
    {
        if (tcp_flag == 1)
        {
        seekfree_assistant_oscilloscope_data.data[0] = current_speed_l;//current_speed_l;
        seekfree_assistant_oscilloscope_data.data[1] = current_speed_r;//current_speed_r;
        seekfree_assistant_oscilloscope_data.data[2] = Left_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[3] = Right_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[4] = current_speed;
        seekfree_assistant_oscilloscope_data.data[5] = speed_setPoint;
        seekfree_assistant_oscilloscope_data.data[6] = speed_l_integral;
        seekfree_assistant_oscilloscope_data.data[7] = speed_r_integral;

        // ���ñ�����Ҫ���ͼ���ͨ��������
        seekfree_assistant_oscilloscope_data.channel_num = 8;

        // �����������4��ͨ�������ݣ����֧��8ͨ��
        seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        }

        if (break_flag == 0)
        {
            if (speed_flag == 0)
            {
                pid_output_l = pwm;
                pid_output_r = pwm;
            }
            else
            {
                if (zhushi_flag)
                    printf("current_speed:%lf\r\n", current_speed);
                if (zhushi_flag)
                    printf("speed_l_setPoint:%d\r\n", speed_l_setPoint);
                if (zhushi_flag)
                    printf("speed_r_setPoint:%d\r\n", speed_r_setPoint);
                
                // 差速在pwm
                // speed_setPoint_l = speed_setPoint;
                // speed_setPoint_r = speed_setPoint;

                //分开给速度环
                pid_speed_output_l = SpeedLPID_Calculate_wei(speed_setPoint_l, Left_encoder_data_kal);
                pid_speed_output_r = SpeedRPID_Calculate_wei(speed_setPoint_r, Right_encoder_data_kal);

                //差速在速度环
                pid_output_l = pid_speed_output_l;
                pid_output_r = pid_speed_output_r;

                // //一起给速度环
                // pid_speed_output_l = SpeedLPID_Calculate_wei(speed_setPoint, current_speed);
                // pid_speed_output_r= pid_speed_output_l;

                if (zhushi_flag)
                    printf("pid_output_l:%lf\r\n", pid_output_l);
                if (zhushi_flag)
                    printf("pid_output_r:%lf\r\n", pid_output_r);
            }
            control_motors(pid_output_l, pid_output_r);
        }
        else if (break_flag == 1)
        {

        
            if (speed_flag == 1)
            {
                //位置式 分开控制
                pid_output_l = SpeedLPID_Calculate_wei(0, Left_encoder_data_kal);
                pid_output_r = SpeedRPID_Calculate_wei(0, Right_encoder_data_kal);
                control_motors(pid_output_l, pid_output_r);

            }
            else if (speed_flag == 0)
            {
                // 直接停止电机，并重置目标速度
                target_speed = 0.0f;
                control_motors(0, 0);
            }
        }
    }
    else if(!motor_test_flag)
    {
        control_motors(0, 0);
    }
    else if(motor_test_flag)
    {
        if (tcp_flag == 1)
        {
        seekfree_assistant_oscilloscope_data.data[0] = current_speed_l;//current_speed_l;
        seekfree_assistant_oscilloscope_data.data[1] = current_speed_r;//current_speed_r;
        seekfree_assistant_oscilloscope_data.data[2] = Left_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[3] = Right_encoder_data_kal;
        seekfree_assistant_oscilloscope_data.data[4] = current_speed;
        seekfree_assistant_oscilloscope_data.data[5] = speed_setPoint;
        seekfree_assistant_oscilloscope_data.data[6] = speed_l_integral;
        seekfree_assistant_oscilloscope_data.data[7] = speed_r_integral;
        //        detector_oscilloscope_data.data[7] = 10000;

        // ���ñ�����Ҫ���ͼ���ͨ��������
        seekfree_assistant_oscilloscope_data.channel_num = 8;

        // �����������4��ͨ�������ݣ����֧��8ͨ��
        seekfree_assistant_oscilloscope_send(&seekfree_assistant_oscilloscope_data);
        }
    }

}